#include <Arduino.h>
#include "BTSerial.h"
#include "setting_mode.h"
#include "PS3_contrl.h"
//#include "linkit_control/ui_controls.h"
//#include "hexapod/debug.h"
#include "config.h"
#include "hexapod.h"
#include "ps2.h"
#define REACT_DELAY hexapod::config::movementInterval
extern char ps3_enable;
unsigned char flag;
 unsigned char flag1=0x30;	
void normal_loop(void) {

  // check if we are connect by some 
  // BLE central device, e.g. an mobile app

  auto t0 = millis();

  // Process the incoming BLE write request
  // and translate them to control events
 
   if(ps2_enable==1) 
   	{
   	flag=0x30;
      if(check_serial_BT()!=0) ps2_enable=0 ; // btnGroup.getPressFlag();
        }else if(ps3_enable!=1){
          flag =check_serial_BT();
          }else {
        flag=check_command().c_str()[0];
            }
   if(flag=='s')
  	setting_loop();
  if(flag!=flag1 && flag!=0) flag1=flag;
  
  auto mode = hexapod::MOVEMENT_STANDBY;
  //Serial.println(flag1);
  for (auto m = hexapod::MOVEMENT_STANDBY; m < hexapod::MOVEMENT_TOTAL; m++) {
    if ((flag1-0x30) == m) {
      mode = m;
      Serial.println("mode");
      Serial.println(mode);
      break;

    }
  }

//flag=0;
        
  
  hexapod::Hexapod.processMovement(mode, REACT_DELAY);

  auto spent = millis() - t0;

  if(spent < REACT_DELAY) {
    // Serial.println(spent);
    delay(REACT_DELAY-spent);
	
  }
  else {
   // Serial.println(spent);
  }
}